#!/usr/bin/python
import roslib; roslib.load_manifest('DelphinROSv2')
import rospy
import serial
import time
import numpy
import math
import string
from pylab import *
from DelphinROSv2.msg import gps
from DelphinROSv2.msg import status

################### GLOBAL VARIABLES ################### 

global serialPort
global identifier

global latitude
global lat_NorS
global longitude
global long_WorE
global time
global number_of_satelites
global fix
global speed #in knots

################### SERIAL SETUP ################### 

def setUpSerial():
    global serialPort
    serialPort = serial.Serial(port='/dev/usbgps', baudrate='4800', timeout=0.1) # may need to change timeout if having issues!
    serialPort.bytesize = serial.EIGHTBITS
    serialPort.stopbits = serial.STOPBITS_ONE
    serialPort.parity = serial.PARITY_NONE
    print "Initialised GPS Serial."
    print serialPort.portstr
    print serialPort.isOpen()
    return serialPort.isOpen()

################### Functions to process different types of incoming message ###################  

def gpgga(string):
    global latitude
    global lat_NorS
    global longitude
    global long_WorE
    global time
    global number_of_satelites
    global fix

    if string[1]:
        time = float(string[1])
    latitude = string[2]
    lat_NorS = string[3]
    longitude = string[4]
    long_WorE = string[5]
    if string[7]:
        number_of_satelites = int(string[7])
    if string[6]:    
	fix = int(string[6])

def gpgsa(string):
    a=0 #Incoming message contains details of satelites IDs.  This is currently of no use to us so will be ignored

def gprmc(string):
    global latitude
    global lat_NorS
    global longitude
    global long_WorE
    global time
    global speed #in knots

    if string[1]:
        time = float(string[1])
    latitude = string[3]
    lat_NorS = string[4]
    longitude = string[5]
    long_WorE = string[6]

    if string[7]:
        speed = float(string[7])

    #magnetic_variation = float(string[10])		#Information doesnt appear to be received, perhaps a different type of GPS is needed
    #magvar_WorE = string[11]       

def gpgsv(string):
    global number_of_satelites
    if len(string[3]) == 1:
	number_of_satelites = int(string[3])

################### MAIN LOOP ################### 

def validDataCheck():
    global serialPort
    global identifier
    
    while not serialPort.read(1) == '$':
        pass
    data = serialPort.readline(size = None, eol = '\n')		#Read in line of data
    split_data = string.split(data,',')				#Split message by comma separator
    print 'DataCHECK: ', data

    message_identifier = split_data[0]	
    if message_identifier in identifier:
        return True
    else:
        return False    

def listenForData():

    #Establish Global Variables
    global	latitude
    global      lat_NorS
    global      longitude
    global      long_WorE
    global      time	
    global      number_of_satelites 
    global      fix
    global      speed

    #Initialise values to zero - important as different messages contain different values
    latitude		=	'0'	
    latitude_decimal	=	0
    lat_NorS		=	'0'
    longitude		=	'0'
    longitude_decimal	=	0
    long_WorE		=	'0'
    time		=	0
    number_of_satelites =	0
    fix			=	0
    speed 		= 	0
    X			= 	0
    Y			= 	0

    first_reading = True
    mean_earth_radius = 6370973.27862					#metres
            
    while not rospy.is_shutdown():
        try:
            while serialPort.inWaiting() > 0 and serialPort.read(1) == '$': #while there is data to be read - read the line...
                        
                data = serialPort.readline(size = None, eol = '\n')		#Read in line of data
                split_data = string.split(data,',')				#Split message by comma separator
                print 'Data: ', data

                message_identifier = split_data[0]				#Message identifier is the first part of the message and defines message format
                identifier[message_identifier](split_data)			#Process message according to indentifier type

                #### Function to convert latitude to decimal degrees ####
                if len(latitude) == 9:
                    minutes = numpy.float64(latitude[2:9])/60                   #numpy.float64 inserted instead of standard float
                    degrees = int(latitude[0:2])
                    latitude_decimal = degrees+minutes
                    if lat_NorS == 'S':
                        latitude_decimal = -latitude_decimal

                #### Function to convert longitude to decimal degrees ####
                if len(longitude) == 10:
                    minutes = numpy.float64(longitude[3:10])/60                 #numpy.float64 inserted instead of standard float
                    degrees = int(longitude[0:3])
                    longitude_decimal = degrees+minutes
                    if long_WorE == 'W':
                        longitude_decimal = -longitude_decimal

                #### Set origin for co-ordinates from first satelite reading ####
                if first_reading == True and len(latitude) == 9 and len(longitude) == 10:
                    lat_orig = 50.9567
                    long_orig = -1.36735
                    first_reading = False

                #### Calculate X Y co-ordinates from original position ####
                if first_reading == False and len(latitude) == 9 and len(longitude) == 10:
                    delta_lat = latitude_decimal - lat_orig
                    delta_long = longitude_decimal - long_orig 
                    X = math.radians(delta_lat)*mean_earth_radius
                    Y = math.radians(delta_long)*mean_earth_radius
		if number_of_satelites >=4:
                
                #Publish data to compass_out
                	pub.publish(latitude = latitude_decimal, longitude = longitude_decimal, time = time, number_of_satelites = number_of_satelites, fix = fix, speed = speed, x = X, y = Y)
        except:
            print 'read error'
################### SHUTDOWN FUNCTION ################### 

def shutdown():
    serialPort.flushInput()
    serialPort.flushOutput()
    pubStatus.publish(nodeID = 4, status = False)
    serialPort.close()

################### INITIALISING FUNCTION ################### 

if __name__ == '__main__':
    time.sleep(2) #Allow System to come Online    
    global identifier
    global pub    
    rospy.init_node('gps_sensor')
    rospy.on_shutdown(shutdown)         #Defining shutdown behaviour  
       
    identifier = {'GPGGA' : gpgga,
                'GPGSA' : gpgsa,
                'GPRMC' : gprmc,
                'GPGSV' : gpgsv,
    }
  
    #Define Publishers
    pubStatus = rospy.Publisher('status', status)
    pub = rospy.Publisher('gps_out', gps) 
    time.sleep(1)
    #Setup serial port and check its status
    port_status = setUpSerial()    
    str = "GPS port status = %s. Port = %s" %(port_status, serialPort.portstr)
    rospy.loginfo(str)
    
    #Initial Data Test
    
    string_status=validDataCheck()
    
    if (port_status and string_status) == True:    
        pubStatus.publish(nodeID = 4, status = True)
        rospy.loginfo("GPS online")
    else:
        pubStatus.publish(nodeID = 4, status = False)    


    listenForData()                     #Main loop for receiving data

    
